#include "agent.h"
#include "graphics.h"
#include "mathutil.h"
#include "C_Application.h"

extern int screenWidth;
extern int screenHeight;

Agent::Agent()
{

}

void Agent::Tick(float fDeltaTime)
{
    UpdateAI(fDeltaTime);
    UpdatePhysics(fDeltaTime);
}

void Agent::UpdateAI(float fDeltaTime)
{
	static float s_fMinCollideTime = 1.f;
	static float s_fTurnAngularSpeed = 90.f;

	if(GetSpeed().IsZero())
		return;

    vector<Agent*>& agentArrayRef = C_Application::GetInstance()->GetAgentArray();

	float minCollideTime = s_fMinCollideTime;
	Agent* collideAgent = NULL;

    for(vector<Agent*>::iterator iter = agentArrayRef.begin(); iter != agentArrayRef.end(); ++iter )
    {
        if((*iter) != this)
        {
			float collideTime = AnticipateCollideTime(*iter);
			if(collideTime > 0.f && collideTime < minCollideTime)
			{
				minCollideTime = collideTime;
				collideAgent = *iter;
			}
        }
    }

    vec3 currentVelocity = GetSpeed();
    float theta = fDeltaTime * s_fTurnAngularSpeed * PI / 180.f;

	if(collideAgent)
	{
		vec3 v = GetSpeed() - collideAgent->GetSpeed();	// relative velocity
		vec3 posOffset = collideAgent->GetPosition() - GetPosition();

		vec3 cross = v.Cross(posOffset);
		if(cross.Z > 0.f)
		{
			theta *= -1.f;
		}

		vec3 newVelocity;
		newVelocity.X = currentVelocity.X * cos(theta) - currentVelocity.Y * sin(theta);
		newVelocity.Y = currentVelocity.X * sin(theta) + currentVelocity.Y * cos(theta);
		SetSpeed(newVelocity);
	}
    else if(m_vSpeed.Y == 0.f || abs(m_vSpeed.X / m_vSpeed.Y) > 3.f || abs(m_vSpeed.X / m_vSpeed.Y) < 0.333f) 
    {
        vec3 newVelocity;
        newVelocity.X = currentVelocity.X * cos(theta) - currentVelocity.Y * sin(theta);
        newVelocity.Y = currentVelocity.X * sin(theta) + currentVelocity.Y * cos(theta);
        SetSpeed(newVelocity);
    }
}

void Agent::UpdatePhysics(float fDeltaTime)
{
    m_vPosition += m_vSpeed * fDeltaTime;
    
    if(m_vPosition.X - m_fRadius < 0.f && m_vSpeed.X < 0.f)
        m_vSpeed.X *= -1.f;

    if(m_vPosition.X + m_fRadius > screenWidth && m_vSpeed.X > 0.f)
        m_vSpeed.X *= -1.f;

    if(m_vPosition.Y - m_fRadius < 0.f && m_vSpeed.Y < 0.f)
        m_vSpeed.Y *= -1.f;

    if(m_vPosition.Y + m_fRadius > screenHeight && m_vSpeed.Y > 0.f)
        m_vSpeed.Y *= -1.f;
}

void Agent::Render()
{
    static int g_iDrawCircleNumOfSide = 20;

    vec3 vLastDrawPoint = m_vPosition + vec3(m_fRadius * cos(0.f), m_fRadius * sin(0.f), 0.f);

    for(int i = 0; i < g_iDrawCircleNumOfSide; i++)
    {
        float fDrawPointAngle = 2.0f * PI * ( (float)(i+1) / (float)g_iDrawCircleNumOfSide );
        vec3 vDrawPoint = m_vPosition + vec3(m_fRadius * cos(fDrawPointAngle), m_fRadius * sin(fDrawPointAngle), 0.f);

        DrawLine( vLastDrawPoint, vDrawPoint, m_Color );

        vLastDrawPoint = vDrawPoint;
    }
}

float Agent::AnticipateCollideTime(Agent* o)
{
	float time = -1.f;

	vec3 v = GetSpeed() - o->GetSpeed();	// relative velocity
	vec3 pa = GetPosition();
	vec3 pb = o->GetPosition();
	
	float a = v.X * v.X + v.Y * v.Y;
	float b = 2 * (v.X * (pa.X - pb.X) + v.Y * (pa.Y - pb.Y));
	float c = (pa.X - pb.X) * (pa.X - pb.X) + (pa.Y - pb.Y) * (pa.Y - pb.Y) - (GetRadius() + o->GetRadius()) * (GetRadius() + o->GetRadius()) * 2.f;

	float delta = b * b - 4 * a * c;
	if(delta >= 0)
	{
		time = (-b - sqrt(delta)) / (2 * a);
	}

	return time;
}